// Interrupts are generated every 20ms because
// Roll pin from transmitter triggers interrupt (50MHz, PWM)
// Modulo division by 5 causes each time slot to happen every 100ms

void interruptHandler() {
  switch ((timeSlot += 1) % 5) {
  case 0:
    // Read Receiver
    // This has been tested with AR6100 and AR6200 Spektrum receivers
    roll = pulseIn(ROLLPIN, HIGH, TIMEOUT);
    aux = pulseIn(AUXPIN, HIGH, TIMEOUT);
    mode = pulseIn(MODEPIN, HIGH, TIMEOUT);
    pitch = pulseIn(PITCHPIN, HIGH, TIMEOUT);
    throttle = pulseIn(THROTTLEPIN, HIGH, TIMEOUT);
    yaw = pulseIn(YAWPIN, HIGH, TIMEOUT);   
    // Calculate Commanded Angles
    // Reduce transmitter commands using xmitFactor and center around 1500
    // rollCommand, pitchCommand and yawCommand used in main loop of MikroQuad.pde to control quad
    rollCommand = ((roll - 1500) * xmitFactor) + 1500;
    pitchCommand = ((pitch - 1500) * xmitFactor) + 1500;
    yawCommand = ((yaw - 1500) * xmitFactor) + 1500;
    break;
  case 2:
    // Send Data
    switch (queryType) {
    case 'X': // send no debug messages
      break;
    case 'A': // send all data
      Serial.print(deltaTime);
      comma();
      Serial.print(rollGyroSensor);
      comma();
      Serial.print(pitchGyroSensor);
      comma();
      Serial.print(yawGyroSensor);
      comma();
      Serial.print(throttle);
      comma();
      Serial.print(rollMotorCommand);
      comma();
      Serial.print(pitchMotorCommand);
      comma();
      Serial.print(yawMotorCommand);
      comma();
      Serial.print(frontCommand);
      comma();
      Serial.print(rearCommand);
      comma();
      Serial.print(rightCommand);
      comma();
      Serial.print(leftCommand);
      comma();
      Serial.print(armed, BIN);
      comma();
      Serial.println(mode);
      break;
    case 'S': // send sensor data
      Serial.print(rollGyroSensor);
      comma();
      Serial.print(pitchGyroSensor);
      comma();
      Serial.print(yawGyroSensor);
      comma();
      Serial.print(rollAccSensor);
      comma();
      Serial.print(pitchAccSensor);
      comma();
      Serial.print(zAccSensor);
      comma();
      Serial.print(rollLevelAdjust);
      comma();
      Serial.print(pitchLevelAdjust);
      comma();
      Serial.print(dtostrf(rollAngle, 1, 1, string));
      comma();
      Serial.print(dtostrf(pitchAngle, 1, 1, string));
      Serial.println();
      break;
    case 'D': // raw read sensor data
      Serial.print(analogRead(ROLLRATEPIN));
      comma();
      Serial.print(analogRead(PITCHRATEPIN));
      comma();
      Serial.print(analogRead(YAWRATEPIN));
      comma();
      Serial.print(analogRead(ROLLACCELPIN));
      comma();
      Serial.print(analogRead(PITCHACCELPIN));
      comma();
      Serial.println(analogRead(ZACCELPIN));
      break;
    case 'U': // send user defined values
      for (int i=0; i<3; i++) {
        Serial.print(dtostrf(PID[i], 1, 2, string));
        comma();
      }
      for (int i=0; i<3; i++) {
        Serial.print(dtostrf(levelPID[i], 1, 2, string));
        comma();
      }
      for (int i=0; i<3; i++) {
        Serial.print(dtostrf(yawPID[i], 1, 2, string));
        comma();
      }
      Serial.print(dtostrf(windupGuard, 1, 2, string));
      comma();
      Serial.print(dtostrf(levelLimit, 1, 2, string));
      comma();
      Serial.print(dtostrf(levelInterval, 1, 2, string));
      comma();
      Serial.print(dtostrf(xmitFactor, 1, 2, string));
      comma();
      Serial.print(dtostrf(gyroSmoothFactor, 1, 2, string));
      comma();
      Serial.print(dtostrf(accSmoothFactor, 1, 2, string));
      comma();
      Serial.print(dtostrf(timeConstant, 1, 3, string));
      Serial.println(); // will probably add more responses in the future
      queryType = 'X';
      break;
    case 'T': // read processed transmitter values
      Serial.print(dtostrf(xmitFactor, 1, 2, string));
      comma();
      Serial.print(rollCommand);
      comma();
      Serial.print(pitchCommand);
      comma();
      Serial.print(yawCommand);
      comma();
      Serial.print(rollLevelAdjust);
      comma();
      Serial.print(pitchLevelAdjust);
      comma();
      Serial.print(rollMotorCommand);
      comma();
      Serial.print(pitchMotorCommand);
      comma();
      Serial.println(yawMotorCommand);
      break;
    case 'R': // send receiver values
      Serial.print(throttle);
      comma();
      Serial.print(roll);
      comma();
      Serial.print(pitch);
      comma();
      Serial.print(yaw);
      comma();
      Serial.print(mode);
      comma();
      Serial.println(aux);
      break;
    }
    break;
  default:
    break;
  }
}

void comma() {
  Serial.print(',');
}
